/*
 * @Description: 通过ros发布点云
 * @Author: Zhijian Qiao
 * @Date: 2020-02-05 02:27:30
 */

#include "publisher/image_publisher.hpp"
#include "glog/logging.h"

namespace avp_slam {
    ImagePublisher::ImagePublisher(ros::NodeHandle &nh,
                                   std::string topic_name,
                                   size_t buff_size)
            : nh_(nh) {
        it = std::make_shared<image_transport::ImageTransport>(nh_);
        publisher_ = it->advertise(topic_name, buff_size);
    }

    void ImagePublisher::Publish(ImageData::IMAGE &image_input, double time) {
        ros::Time ros_time(time);
        PublishData(image_input, ros_time);
    }

    void ImagePublisher::Publish(ImageData::IMAGE &image_input) {
        ros::Time time = ros::Time::now();
        PublishData(image_input, time);
    }

    void ImagePublisher::PublishData(ImageData::IMAGE &image_input, ros::Time time) {
        msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_input).toImageMsg();
        msg->header.stamp = time;
        publisher_.publish(msg);
    }

    bool ImagePublisher::HasSubscribers() {
        return publisher_.getNumSubscribers() != 0;
    }
} // namespace avp_slam